;----------------------------------------------------------------------------
;  mth_norm.src -- MTH ライブラリ 法線ベクタ計算 モジュール
;  Copyright(c) 1994 SEGA
;  Written by H.E on 1994-06-22 Ver.1.00
;  Updated by H.E on 1994-06-22 Ver.1.00
;
;  このライブラリは法線ベクタ計算モジュールで、
;  以下のルーチンを含む。
;
;  _MTH_ComputeNormalVect       -  法線ベクタ計算関数
;
;----------------------------------------------------------------------------
          .EXPORT     _MTH_ComputeNormVect
;
          .SECTION    CALL_STACK,DUMMY
SAVE_R14  .RES.L      1
NORMAL_P  .RES.L      1
;
          .SECTION    MthXyz,DUMMY
X         .RES.L      1
Y         .RES.L      1
Z         .RES.L      1
;
          .SECTION    SEGA_P,CODE,ALIGN=4
;
;-----------------------------------------------------------------------------
;
; NAME:  MTH_ComputeNormVect()  - Compute Normal Vector
;
;  void MTH_ComputeNormVect(Fixed32 surfNormK,
;			 MthXyz *p0, MthXyz *p1, MthXyz *p2, MthXyz *normal)
;
; PARAMETERS :
;
; 　　(1) Fixed32  surfNormK   - <i>  法線ベクトルの計算補正値
; 　　(2) MthXyz   *p0         - <i>  頂点０
; 　　(3) MthXyz   *p1         - <i>  頂点１
; 　　(4) MthXyz   *p2         - <i>  頂点２
; 　　(5) MthXyz   *normal     - <o>  法線
;
; DESCRIPTION:
;
;     指定３頂点座標より法線ベクトル（単位ベクトル）を計算する
;
;
; POSTCONDITIONS:
;
;
; CAVEATS:
;
;-----------------------------------------------------------------------------
;
	  .ALIGN	4
_MTH_ComputeNormVect:
          MOV.L       R14,@-R15         ; save register
          MOV         R15,R14
          MOV.L       R11,@-R15
          MOV.L       R10,@-R15
          MOV.L       R9,@-R15
          MOV.L       R8,@-R15
          STS.L       PR,@-R15
          STS.L       MACL,@-R15
          STS.L       MACH,@-R15
;
          CMP/PZ      R4                ; R4 : surfNormK
          BT/S        cnv_00            ; if(surfNormK >= 0) goto cnv00
          MOV         #1,R8             ; R8 : sign = 1
          MOV         #-1,R8            ;      sign = -1
          NEG         R4,R4             ; surfNormK *= -1
cnv_00:                                 ; R5 : p0, R6 : p1, R7 : p2
;
          MOV.L       @(Z,R6),R0        ; b0z = (p1->z - p0->z) * surfNormK
          MOV.L       @(Z,R5),R1
          SUB         R1,R0
	  DMULS.L     R0,R4
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0 
          MOV.L       R0,@-R15          ; push b0z
;
          MOV.L       @(Y,R6),R0        ; b0y = (p1->y - p0->y) * surfNormK
          MOV.L       @(Y,R5),R1
          SUB         R1,R0
	  DMULS.L     R0,R4
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0 
          MOV.L       R0,@-R15          ; push b0y
;
          MOV.L       @(X,R6),R0        ; b0x = (p1->x - p0->x) * surfNormK
          MOV.L       @(X,R5),R1
          SUB         R1,R0
	  DMULS.L     R0,R4
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0 
          MOV.L       R0,@-R15          ; push b0x
          MOV         R15,R2            ; @R2 = (b0x, b0y, b0z)
;
          MOV.L       @(Z,R7),R0        ; b1z = (p2->z - p1->z) * surfNormK
          MOV.L       @(Z,R6),R1
          SUB         R1,R0
	  DMULS.L     R0,R4
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0 
          MOV.L       R0,@-R15          ; push b1z
;
          MOV.L       @(Y,R7),R0        ; b1y = (p2->y - p1->y) * surfNormK
          MOV.L       @(Y,R6),R1
          SUB         R1,R0
	  DMULS.L     R0,R4
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0 
          MOV.L       R0,@-R15          ; push b1y
;
          MOV.L       @(X,R7),R0        ; b1x = (p2->x - p1->x) * surfNormK
          MOV.L       @(X,R6),R1
          SUB         R1,R0
	  DMULS.L     R0,R4
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0 
          MOV.L       R0,@-R15          ; push b1x
          MOV         R15,R3            ; @R3 = (b1x, b1y, b1z)
;
          MOV.L       @(Z,R2),R0        ; nx = b0z*b1y - b0y*b1z
          MOV.L       @(Y,R3),R1        ; R9     R9        R4
	  DMULS.L     R0,R1
 	  STS         MACH,R1
 	  STS	      MACL,R9
 	  XTRCT	      R1,R9 
          MOV.L       @(Y,R2),R0
          MOV.L       @(Z,R3),R1
	  DMULS.L     R0,R1
 	  STS         MACH,R1
 	  STS	      MACL,R4
 	  XTRCT	      R1,R4 
          SUB         R4,R9
;
          MOV.L       @(X,R2),R0        ; ny = b0x*b1z - b0z*b1x
          MOV.L       @(Z,R3),R1        ; R10    R10       R4
	  DMULS.L     R0,R1
 	  STS         MACH,R1
 	  STS	      MACL,R10
 	  XTRCT	      R1,R10 
          MOV.L       @(Z,R2),R0
          MOV.L       @(X,R3),R1
	  DMULS.L     R0,R1
 	  STS         MACH,R1
 	  STS	      MACL,R4
 	  XTRCT	      R1,R4 
          SUB         R4,R10
;
          MOV.L       @(Y,R2),R0        ; nz = b0y*b1x - b0x*b1y
          MOV.L       @(X,R3),R1        ; R11    R11       R4
	  DMULS.L     R0,R1
 	  STS         MACH,R1
 	  STS	      MACL,R11
 	  XTRCT	      R1,R11 
          MOV.L       @(X,R2),R0
          MOV.L       @(Y,R3),R1
	  DMULS.L     R0,R1
 	  STS         MACH,R1
 	  STS	      MACL,R4
 	  XTRCT	      R1,R4 
          SUB         R4,R11
;
	  DMULS.L     R9,R9             ; R4 : w = nx*nx + ny*ny + nz*nz
 	  STS         MACH,R1           ;            R9     R10     R11
 	  STS	      MACL,R4
 	  XTRCT	      R1,R4 
	  DMULS.L     R10,R10
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0
          ADD         R0,R4 
	  DMULS.L     R11,R11
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0
          ADD         R0,R4 
;
          MOV         #65,R2           ; R2 = 0.001
          CMP/GE      R2,R4            ; if(w >= 0.001)
          BT          cnv_01           ;    goto cnv_01
          MOV         #65,R4           ; w = 0.001
cnv_01:                             
          MOV.L       FISQRT,R3
          JSR         @R3
          NOP
          MUL.L       R8,R0
          STS         MACL,R4          ; w = FISQRT(w) * sign
;
          MOV.L       @(NORMAL_P,R14),R7     ; @R7 : normal
;
	  DMULS.L     R9,R4            ; normal->x = nx * w
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0
          MOV.L       R0,@(X,R7)
;
	  DMULS.L     R10,R4           ; normal->y = ny * w
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0
          MOV.L       R0,@(Y,R7)
;
	  DMULS.L     R11,R4            ; normal->z = nz * w
 	  STS         MACH,R1
 	  STS	      MACL,R0
 	  XTRCT	      R1,R0
          MOV.L       R0,@(Z,R7)
;
          ADD         #24,R15           ; restore register
          LDS.L       @R15+,MACH
          LDS.L       @R15+,MACL
          LDS.L       @R15+,PR
          MOV.L       @R15+,R8
          MOV.L       @R15+,R9
          MOV.L       @R15+,R10
          MOV.L       @R15+,R11
          RTS
          MOV.L       @R15+,R14
;                             
	  .ALIGN	4
FISQRT	  .DATA.L     _FISQRT
;
;
; C interface:
;	  Fixed32 FISQRT(Fixed32 a);
;
; return:
;	  1/sqrt(a)
;
; method:
;	  Newton-Raphson
;         xn+1 = xn*(3-a*xn*xn)/2
;
; note:   MACL, MACH not saved
;
; pgm:	  1994.05.14 by T. Suzuki (SEGA C/S Hard dpt.)
;
;
_FISQRT:
	XOR	R0,R0
	CMP/PL	R4		; if (a<=0) return 0;
	BF	RETURN
	MOV.L	C_1,R0
	XOR	R6,R6
	MOV.L	C_4,R7
NORMLZ:				; normalize 1/4 <= a < 1
	CMP/GE	R0,R4
	BF	NORMLZ2
	ADD	#1,R6
	BRA	NORMLZ
	SHLR2	R4
NORMLZ2:
	CMP/GE	R4,R7
	BF	NEWTON
	ADD	#-1,R6
	BRA	NORMLZ2
	SHLL2	R4
NEWTON:
	MOV	#3,R5		; only 3 times do Newton
REPEAT:
	DMULS.L	R0,R0
	MOV.L	C_3,R1
	STS	MACL,R2
	STS	MACH,R3
	XTRCT	R3,R2
	DMULS.L	R4,R2
	STS	MACL,R2
	STS	MACH,R3
	XTRCT	R3,R2
	SUB	R2,R1
	DMULS.L	R0,R1
	STS	MACL,R0
	STS	MACH,R3
	XTRCT	R3,R0
	SHAR	R0
	DT	R5
	BF	REPEAT
DENORMLZ:
	TST	R6,R6
	BT	RETURN
	CMP/PL	R6
	BF	DENORML2
	ADD	#-1,R6
	BRA	DENORMLZ
	SHLR	R0
DENORML2:
	ADD	#1,R6
	BRA	DENORMLZ
	SHLL	R0
RETURN:
	RTS
	NOP
;
	.ALIGN	4
C_1	.DATA.L	H'00010000	; const 1
C_3	.DATA.L	H'00030000	; const 3
C_4	.DATA.L	H'00004000	; const 1/4
;
        .END
